package chabernac.gravidroid.model;

import java.util.HashSet;
import java.util.Set;

public class LinearMass implements iMass {
	private Mass mass1;
	private Mass mass2;
	private float restDistance;
	private final float strength;
	private final float speedResisance;
	private final Set<iMass> myParents = new HashSet<iMass>();

	private final Vector myForceVector = new Vector();
	private final Vector myForceMass1 = new Vector();
	private final Vector myForceMass2 = new Vector();
	private final Vector mySpeedMass1 = new Vector();
	private final Vector myAverageSpeed = new Vector();
	private float myForce;

	public LinearMass(Mass mass1, Mass mass2, float strength, float aSpeedResistance) {
		this(mass1, mass2, new Vector(mass1.location, mass2.location).getLength(), strength, aSpeedResistance);
	}

	public LinearMass(Mass mass1, Mass mass2, float restDistance, float strength, float aSpeedResistance) {
		super();
		this.mass1 = mass1;
		this.mass2 = mass2;
		this.restDistance = restDistance;
		this.strength = strength;
		this.speedResisance = aSpeedResistance;
		mass1.addParent(this);
		mass2.addParent(this);
	}

	public void recalculateRestDistances() {
		restDistance = new Vector(mass1.location, mass2.location).getLength();
	}

	@Override
	public void calculateInternalForces() {
		myForceVector.set(mass2.location.x - mass1.location.x, mass2.location.y - mass1.location.y);
		// float theForce = strength * (float)Math.pow((theDistance -
		// restDistance)/10, 3);
		myForce = strength * (myForceVector.getLength() - restDistance);

		// System.out.println("Internal force " + myForce);

		myForceVector.normalize(myForceVector);
		// if (myForce < 0)
		// myForceVector.multiply(-1, myForceVector);
		// theForceVector is now the normalized vector pointing from a to b

		// the resistance to speed is caluculated as the speed in the direction
		// between the two masses multiplied with a factor
		// see also: http://en.wikipedia.org/wiki/Damping

		// first calculate average speed
		myAverageSpeed.set(0, 0);
		myAverageSpeed.add(mass1.speed, myAverageSpeed);
		myAverageSpeed.add(mass2.speed, myAverageSpeed);
		myAverageSpeed.divide(2, myAverageSpeed);

		// calculate relative speed of mass1, i.e. the speed relative to the average
		// speed
		mass1.speed.minus(myAverageSpeed, mySpeedMass1);

		// the dampening force is calculated as the magnitude of the speed in the
		// direction of the force multiplied with a dampening
		// factor
		myForce -= speedResisance * myForceVector.dotProduct(mySpeedMass1);
		// System.out.println("Internal force after dampening" + myForce);

		// limit the force
		myForce = limitForce(myForce);

		// myForceVector is still a normalized vector pointing from mass1 to mass2,
		// now multiply it with the size of the force
		myForceVector.multiply(myForce, myForceMass1);
		myForceVector.multiply(-myForce, myForceMass2);

		// add the calculated for the total force vector of each mass
		mass1.force.add(myForceMass1, mass1.force);
		mass2.force.add(myForceMass2, mass2.force);
	}

	public void calculateIntrusiveForce(Mass aMass) {
		// force from m1 to mass
		myForceMass1.set(aMass.location.x - mass1.location.x, aMass.location.y - mass1.location.y);

		// force from m2 to mass
		myForceMass2.set(aMass.location.x - mass2.location.x, aMass.location.y - mass2.location.y);

		// length between m1 and m2
		myForceVector.set(mass2.location.x - mass1.location.x, mass2.location.y - mass1.location.y);

		// Vector theVectorFromM1ToMass = new Vector(mass1.location,
		// aMass.location);
		// Vector theVectorFromM2ToMass = new Vector(mass2.location,
		// aMass.location);
		// Vector theVectorFromM1ToM2 = new Vector(mass1.location, mass2.location);
		myForce = limitForce(10 * strength * (myForceMass1.getLength() + myForceMass2.getLength() - myForceVector.getLength()));

		myForceMass1.normalize(myForceMass1);
		myForceMass1.multiply(myForce, myForceMass1);

		myForceMass2.normalize(myForceMass2);
		myForceMass2.multiply(myForce, myForceMass2);

		mass1.force.add(myForceMass1, mass1.force);
		mass2.force.add(myForceMass2, mass2.force);

		myForceMass1.invert(myForceMass1);
		myForceMass2.invert(myForceMass2);

		aMass.force.add(myForceMass1, aMass.force);
		aMass.force.add(myForceMass2, aMass.force);

		// Vector theVectorFromM1ToMass = new Vector(mass1.location,
		// aMass.location);
		// Vector theVectorFromM2ToMass = new Vector(mass2.location,
		// aMass.location);
		// Vector theVectorFromM1ToM2 = new Vector(mass1.location, mass2.location);
		// myForce = limitForce(10 * strength
		// * (theVectorFromM1ToMass.getLength() + theVectorFromM2ToMass.getLength()
		// - theVectorFromM1ToM2.getLength()));
		//
		// theVectorFromM1ToMass.normalize(theVectorFromM1ToMass);
		// theVectorFromM1ToMass.multiply(myForce, theVectorFromM1ToMass);
		//
		// theVectorFromM2ToMass.normalize(theVectorFromM2ToMass);
		// theVectorFromM2ToMass.multiply(myForce, theVectorFromM2ToMass);
		//
		// mass1.force.add(theVectorFromM1ToMass, mass1.force);
		// mass2.force.add(theVectorFromM2ToMass, mass2.force);
		//
		// theVectorFromM1ToMass.invert(theVectorFromM1ToMass);
		// theVectorFromM2ToMass.invert(theVectorFromM2ToMass);
		// aMass.force.add(theVectorFromM1ToMass, aMass.force);
		// aMass.force.add(theVectorFromM2ToMass, aMass.force);
	}

	private float limitForce(float aForce) {
		if (aForce > 3000000) {
			return 3000000;
		}
		if (aForce < -3000000) {
			return -3000000;
		}
		return aForce;
	}

	public Mass getMass1() {
		return mass1;
	}

	public Mass getMass2() {
		return mass2;
	}

	public void setMass1(Mass aMass1) {
		mass1 = aMass1;
	}

	public void setMass2(Mass aMass2) {
		mass2 = aMass2;
	}

	@Override
	public void calculateForce(Mass aMass) {
		ForceCalculator.calculateForce(aMass, this);
	}

	@Override
	public void calculateForce(LinearMass aMass) {
		ForceCalculator.calculateForce(this, aMass);
	}

	@Override
	public void calculateForce(Structure aMass) {
		ForceCalculator.calculateForce(this, aMass);
	}

	@Override
	public void calculateForce(iMass aMass) {
		aMass.calculateForce(this);
	}

	@Override
	public boolean isMoveable() {
		return true;
	}

	@Override
	public void calculateForce(LineForce aMass) {
		ForceCalculator.calculateForce(aMass, this);
	}

	@Override
	public Rectangle getBoxingRect() {
		return Rectangle.constructFrom2Points(mass1.location, mass2.location);
	}

	@Override
	public Set<Mass> getInteralMasses() {
		Set<Mass> theInternalMasses = new HashSet<Mass>();
		theInternalMasses.add(mass1);
		theInternalMasses.add(mass2);
		return theInternalMasses;
	}

	public Line getLine() {
		return new Line(mass1.location, mass2.location);
	}

	public Vector getVector() {
		return new Vector(mass1.location, mass2.location);
	}

	public Line getPreviousLine() {
		return new Line(mass1.previousLocation, mass2.previousLocation);
	}

	@Override
	public Set<iMass> getParents() {
		return myParents;
	}

	public void addParent(iMass aParent) {
		myParents.add(aParent);
	}

	public float getStrength() {
		return strength;
	}

	public float getSpeedResistance() {
		return speedResisance;
	}

	@Override
	public void resetCollisions() {
	}

    @Override
    public Vector getCenterPoint() {
        Vector theCenterPoint = new Vector();
        mass1.location.add( mass2.location, theCenterPoint);
        theCenterPoint.divide( 2, theCenterPoint );
        return theCenterPoint;
    }
}
